#include <stdio.h>
#include <pthread.h>
#include "AlgoChannel.h"
#include "rk_mpi_vpss.h"
#include "rk_mpi_mb.h"
#include "rk_mpi_cal.h"
#include "rk_mpi_vgs.h"
#include "rk_mpi_tde.h"
#include "rk_mpi_sys.h"
#include "rk_mpi_venc.h"
#include "RgaUtils.h"
#include "rga.h"
#include "im2d.h"
#include <vector>
#include <unistd.h>
#include <sys/prctl.h>
#include <string.h>
#include <stdlib.h>
#include <algorithm>

static VGS_DRAW_LINE_S lines[100];

static unsigned char *LoadData(FILE *fp, size_t ofst, size_t sz)
{
    unsigned char *data;
    int ret;

    data = NULL;

    if (NULL == fp)
    {
        return NULL;
    }

    ret = fseek(fp, ofst, SEEK_SET);
    if (ret != 0)
    {
        printf("blob seek failure.\n");
        return NULL;
    }

    data = (unsigned char *)malloc(sz);
    if (data == NULL)
    {
        printf("buffer malloc failure.\n");
        return NULL;
    }
    ret = fread(data, 1, sz, fp);
    return data;
}

static unsigned char *LoadModel(const char *filename, int *model_size)
{
    FILE *fp;
    unsigned char *data;

    fp = fopen(filename, "rb");
    if (NULL == fp)
    {
        printf("Open file %s failed.\n", filename);
        return NULL;
    }

    fseek(fp, 0, SEEK_END);
    int size = ftell(fp);

    data = LoadData(fp, 0, size);

    fclose(fp);

	printf("model size = %d\n", size);
	
    *model_size = size;
    return data;
}

/******************结果**********************/
std::vector<BBOX> mapToLayers(const std::vector<BBOX>& detections, int imageHeight) {
    std::vector<BBOX> mappedDetections;
    for (const auto& detection : detections) {
        int score = detection.score;
        int id = detection.id;
        int x1 = detection.rect.ltX, y1 = detection.rect.ltY, x2 = detection.rect.rbX, y2 = detection.rect.rbY;
        int centerY = (y1 + y2) / 2;

        if (centerY < imageHeight) {
            Rect rect;
            rect.ltX = std::max(0, x1);
            rect.ltY = std::max(0, y1);
            rect.rbX = std::max(0, x2);
            rect.rbY = std::max(0, y2);
            mappedDetections.push_back(BBOX{rect, score, id});
        }
        else if (centerY < 2 * imageHeight) {
            Rect rect;
            rect.ltX = std::max(0, x1 + 2360);
            rect.ltY = std::max(0, y1 - imageHeight);
            rect.rbX = std::max(0, x2 + 2360);
            rect.rbY = std::min(1080, y2 - imageHeight);
            mappedDetections.push_back(BBOX{rect, score, id});
        }
        else {
            Rect rect;
            rect.ltX = std::max(0, x1 + 4720);
            rect.ltY = std::max(0, y1 - 2 * imageHeight);
            rect.rbX = std::max(0, x2 + 4720);
            rect.rbY = std::min(1080, y2 - 2 * imageHeight);
            mappedDetections.push_back(BBOX{rect, score, id});
        }
    }
    return mappedDetections;
}

//判断是否在区域内
bool isIntersecting(const BBOX& box1, const BBOX& box2) {
    int centerX = (box1.rect.ltX + box1.rect.rbX) / 2;
    int centerY = (box1.rect.ltY + box1.rect.rbY) / 2;

    return !(box1.rect.ltX >= box2.rect.rbX || box1.rect.rbX <= box2.rect.ltX);
    //return (centerX >= box2.rect.ltX && centerX <= box2.rect.rbX &&
       // centerY >= box2.rect.ltY && centerY <= box2.rect.rbY);
}

std::vector<BBOX> findIntersectingRectangles(const std::vector<BBOX>& rectangles, const BBOX& region1, const BBOX& region2) {
    std::vector<BBOX> intersectingRectangles;
    for (const auto& box : rectangles) {
        if (isIntersecting(box, region1) || isIntersecting(box, region2)) {
            intersectingRectangles.push_back(box);
        }
    }
    return intersectingRectangles;
}

int intersectionOverUnion(const BBOX& box1, const BBOX& box2) {
    int x1 = std::max(box1.rect.ltX, box2.rect.ltX);
    int y1 = std::max(box1.rect.ltY, box2.rect.ltY);
    int x2 = std::min(box1.rect.rbX, box2.rect.rbX);
    int y2 = std::min(box1.rect.rbY, box2.rect.rbY);

    int intersectionArea = std::max(0, x2 - x1 + 1) * std::max(0, y2 - y1 + 1);
    return intersectionArea;
}

std::pair<double, double> calculateOverlap(const BBOX& box1, const BBOX& box2) {
    int areaBox1 = (box1.rect.rbX - box1.rect.ltX + 1) * (box1.rect.rbY - box1.rect.ltY + 1);
    int areaBox2 = (box2.rect.rbX - box2.rect.ltX + 1) * (box2.rect.rbY - box2.rect.ltY + 1);

    int overlapArea = intersectionOverUnion(box1, box2);

    double iou1 = static_cast<double>(overlapArea) / areaBox1;
    double iou2 = static_cast<double>(overlapArea) / areaBox2;

    return { iou1, iou2 };
}

std::vector<BBOX> remove_duplicate_boxes(const std::vector<BBOX>& boxes, double threshold = 0.4) {
    std::vector<BBOX> duplicate_boxes;

    for (size_t i = 0; i < boxes.size(); ++i) {
        for (size_t j = i + 1; j < boxes.size(); ++j) {
            std::pair<double, double> ious = calculateOverlap(boxes[i], boxes[j]);
            double iou1 = ious.first;
            double iou2 = ious.second;
            //printf("%lf==%lf\n",iou1,iou2);

            if (iou1 > threshold || iou2 > threshold) {
                int area_box1 = (boxes[i].rect.rbX - boxes[i].rect.ltX + 1) * (boxes[i].rect.rbY - boxes[i].rect.ltY + 1);
                int area_box2 = (boxes[j].rect.rbX - boxes[j].rect.ltX + 1) * (boxes[j].rect.rbY - boxes[j].rect.ltY + 1);

                if (area_box1 < area_box2) {
                    duplicate_boxes.push_back(boxes[i]);
                }
                else {
                    duplicate_boxes.push_back(boxes[j]);
                }
            }
            else if(boxes.size() == 2)
            {
                duplicate_boxes.push_back(boxes[i]);
                duplicate_boxes.push_back(boxes[j]);
            }
        }
    }

    return duplicate_boxes;
}


std::vector<BBOX> add_duplicate_boxes(const std::vector<BBOX>& boxes, double threshold = 0.4) {
    std::vector<BBOX> duplicate_boxes;
    if(boxes.size() != 2)
    {
        return duplicate_boxes;
    }
    std::pair<double, double> ious = calculateOverlap(boxes[0], boxes[1]);
    double iou1 = ious.first;
    double iou2 = ious.second;
    if (iou1 <= threshold && iou2 <= threshold) {
        BBOX tmp;
        tmp.rect.ltX = (boxes[0].rect.ltX > boxes[1].rect.ltX) ? boxes[1].rect.ltX : boxes[0].rect.ltX;
        tmp.rect.ltY = (boxes[0].rect.ltY > boxes[1].rect.ltY) ? boxes[1].rect.ltY : boxes[0].rect.ltY;
        tmp.rect.rbX = (boxes[0].rect.rbX > boxes[1].rect.rbX) ? boxes[0].rect.rbX : boxes[1].rect.rbX;
        tmp.rect.rbY = (boxes[0].rect.rbY > boxes[1].rect.rbY) ? boxes[0].rect.rbY : boxes[1].rect.rbY;
        tmp.id = boxes[0].id;
        tmp.score = boxes[0].score;
	if(tmp.rect.rbX < tmp.rect.ltX + 2960)
            duplicate_boxes.push_back(tmp);
	else
	{
	    duplicate_boxes.push_back(boxes[0]);
	    duplicate_boxes.push_back(boxes[1]);
	}
    }

    return duplicate_boxes;
}

bool isBoxEqual(const BBOX& box1, const BBOX& box2) {
    return (box1.rect.ltX == box2.rect.ltX && box1.rect.ltY == box2.rect.ltY && box1.rect.rbX == box2.rect.rbX && box1.rect.rbY == box2.rect.rbY);
}

std::vector<BBOX> AlgoChannel::CalResult(std::vector<BBOX>& detections,int ModelWidth,int ModelHeight)
{
    int inputWidth = 2960;
    int inputHeight = 3240;
    int imageWidth = 7680;
    int imageHeight = 1080;//图像宽度

    
    int offsetX = (640 - ModelWidth) / 2;
    int offsetY = (640 - ModelHeight) / 2;
    if(!detections.empty()){
        for(auto& box : detections){
            //printf("CalResult [%d,%d][%d,%d]\n",box.rect.ltX,box.rect.ltY,box.rect.rbX,box.rect.rbY);
            box.rect.ltX = (float)(box.rect.ltX - offsetX) / (float)ModelWidth * inputWidth;
            box.rect.ltY = (float)(box.rect.ltY - offsetY) / (float)ModelHeight * inputHeight;
            box.rect.rbX = (float)(box.rect.rbX - offsetX) / (float)ModelWidth * inputWidth;
            box.rect.rbY = (float)(box.rect.rbY - offsetY) / (float)ModelHeight * inputHeight;
        }
    }

    std::vector<BBOX> mappedDetections = mapToLayers(detections, imageHeight);

    Rect rect1;
    rect1.ltX = 2360;
    rect1.ltY = 0;
    rect1.rbX = 2960;
    rect1.rbY = 1080;
    BBOX region1 = {rect1, 0, 0};//图像裁剪时的重合区域
    Rect rect2;
    rect2.ltX = 4720;
    rect2.ltY = 0;
    rect2.rbX = 5320;
    rect2.rbY = 1080;
    BBOX region2 = {rect2, 0, 0};//同上

    std::vector<BBOX> intersectingRectangles = findIntersectingRectangles(mappedDetections, region1, region2);//坐标转换
    std::vector<BBOX> delete_boxes = remove_duplicate_boxes(intersectingRectangles);//找到重合区域中重复检测的目标
    std::vector<BBOX> add_boxes = add_duplicate_boxes(intersectingRectangles);//合并重合区域中重复检测的目标
    std::vector<BBOX> filtered_detections;
    for (const auto& box : mappedDetections) {
        if (std::find_if(delete_boxes.begin(), delete_boxes.end(), [&](const BBOX& delete_box) {
            return isBoxEqual(box, delete_box);
            }) == delete_boxes.end()) {
            printf("CalResult [%d,%d][%d,%d] score %d\n",box.rect.ltX,box.rect.ltY,box.rect.rbX,box.rect.rbY,box.id);
            filtered_detections.push_back(box);
        }
    }

    for (const auto& box : add_boxes) {
        printf("CalResult [%d,%d][%d,%d] score %d\n",box.rect.ltX,box.rect.ltY,box.rect.rbX,box.rect.rbY,box.score);
        filtered_detections.push_back(box);
    }
    return filtered_detections;
}


int AlgoChannel::CropYuvData(void* pMBlk,void **pdstMBlk,unsigned int width,unsigned int height)
{
    int count = 0;
    RK_S32 s32Ret = RK_SUCCESS;
    RK_U32 u32taskCount = 0;
    TDE_HANDLE hHandle = 0;

    TDE_SURFACE_S pstSrc;
    TDE_SURFACE_S pstDst;

    TDE_RECT_S recSrc;
    TDE_RECT_S recDst;

    VIDEO_FRAME_INFO_S stVideoFrame;
    MB_PIC_CAL_S stCalResult;

    PIC_BUF_ATTR_S stSrcPicBufAttr;
    stSrcPicBufAttr.u32Width = 7680;
    stSrcPicBufAttr.u32Height = 1080;
    stSrcPicBufAttr.enPixelFormat = RK_FMT_YUV420SP;
    stSrcPicBufAttr.enCompMode = COMPRESS_MODE_NONE;

    if(RK_MPI_CAL_TDE_GetPicBufferSize(&stSrcPicBufAttr, &stCalResult) != RK_SUCCESS)
    {
        RK_LOGE("VGS get pic size");
    }

    MB_BLK pSrcMbBlk = NULL;
    if(RK_MPI_SYS_MmzAlloc(&(pSrcMbBlk),RK_NULL, RK_NULL, stCalResult.u32MBSize) != RK_SUCCESS)
    {
        RK_LOGE("RK_MPI_SYS_MmzAlloc_Cached");
    }
    char* pData = (char *)RK_MPI_MB_Handle2VirAddr(pMBlk);
    char* pSrcData = (char *)RK_MPI_MB_Handle2VirAddr(pSrcMbBlk);
    memcpy(pSrcData,pData,stCalResult.u32MBSize);
    RK_MPI_SYS_MmzFlushCache(pSrcMbBlk, RK_FALSE);
    RK_MPI_MB_SetBufferStride(pSrcMbBlk, 7680, 1080);

    pstSrc.u32Width = 7680;
    pstSrc.u32Height = 1080;
    pstSrc.enColorFmt = RK_FMT_YUV420SP;
    pstSrc.enComprocessMode = COMPRESS_MODE_NONE;
    pstSrc.pMbBlk = pSrcMbBlk;

    PIC_BUF_ATTR_S stPicBufAttr;
    stPicBufAttr.u32Width = width;
    stPicBufAttr.u32Height = height * 3;
    stPicBufAttr.enPixelFormat = RK_FMT_YUV420SP;
    stPicBufAttr.enCompMode = COMPRESS_MODE_NONE;

    if(RK_MPI_CAL_TDE_GetPicBufferSize(&stPicBufAttr, &stCalResult) != RK_SUCCESS)
    {
        RK_LOGE("VGS get pic size");
    }

    if(RK_MPI_SYS_MmzAlloc(pdstMBlk,RK_NULL, RK_NULL, stCalResult.u32MBSize) != RK_SUCCESS)
    {
        RK_LOGE("RK_MPI_SYS_MmzAlloc_Cached");
    }
    RK_MPI_MB_SetBufferStride(*pdstMBlk, width, height * 3);
    pstDst.u32Width = width;
    pstDst.u32Height = height * 3;
    pstDst.enColorFmt = RK_FMT_YUV420SP;
    pstDst.enComprocessMode = COMPRESS_MODE_NONE;
    pstDst.pMbBlk = *pdstMBlk;

    for(int i = 0;i < 3;i++)
    {
        hHandle = RK_TDE_BeginJob();
        if (RK_ERR_TDE_INVALID_HANDLE == hHandle) {
            RK_LOGE("start job fail");
            return RK_FAILURE;
        }
        recSrc.s32Xpos = (width - 600) * i; //重叠拼接
        recSrc.s32Ypos = 0;
        recSrc.u32Width = width;
        recSrc.u32Height = height;

        recDst.s32Xpos = 0;
        recDst.s32Ypos = height * i;
        recDst.u32Width = width;
        recDst.u32Height = height;

        s32Ret = RK_TDE_QuickCopy(hHandle,&pstSrc,&recSrc,&pstDst,&recDst);
        if (s32Ret != RK_SUCCESS) {
            printf("copy cancel job %x\n",s32Ret);
            RK_TDE_CancelJob(hHandle);
            continue;
        }

        s32Ret = RK_TDE_EndJob(hHandle, RK_FALSE, RK_TRUE, 10);
        if (s32Ret != RK_SUCCESS) {
            printf("end cancel job %x\n",s32Ret);
            RK_TDE_CancelJob(hHandle);
            continue;
        }

        RK_TDE_WaitForDone(hHandle);
        RK_TDE_CancelJob(hHandle);
    }

    RK_MPI_SYS_MmzFree(pSrcMbBlk);

    return 0;
}

int AlgoChannel::yuv420ToCvMat(void *ptr, int dstW, int dstH, cv::Mat &image){
    if(!ptr)   return -1;

    int ret;
    im_rect             src_rect;
    im_rect             dst_rect;
    rga_buffer_t        src;
    rga_buffer_t        dst;

    memset(&src_rect, 0, sizeof(src_rect));
    memset(&dst_rect, 0, sizeof(dst_rect));
    memset(&src, 0, sizeof(src));
    memset(&dst, 0, sizeof(dst));

    int dstSize = dstW * dstH * 3;
    unsigned char* pDstData = (unsigned char*) malloc(dstSize);
    memset(pDstData, 0, dstSize);
    
    VIDEO_FRAME_INFO_S *pSrcImg = (VIDEO_FRAME_INFO_S *)ptr;
    src = wrapbuffer_virtualaddr(RK_MPI_MB_Handle2VirAddr(pSrcImg->stVFrame.pMbBlk), pSrcImg->stVFrame.u32Width,
                                 pSrcImg->stVFrame.u32Height, RK_FORMAT_YCrCb_420_SP, pSrcImg->stVFrame.u32VirWidth, pSrcImg->stVFrame.u32VirHeight);
    dst = wrapbuffer_virtualaddr(pDstData, dstW, dstH, RK_FORMAT_RGB_888);
    // printf("pSrcImg size(%d, %d) \n", pSrcImg->stVFrame.u32Width, pSrcImg->stVFrame.u32Height);
    src.format = RK_FORMAT_YCrCb_420_SP;
    dst.format = RK_FORMAT_RGB_888;
    ret = imcheck(src, dst, src_rect, dst_rect);
    if (IM_STATUS_NOERROR != ret) {
        printf("%d, check error! %s \n", __LINE__, imStrError((IM_STATUS)ret));
        return -1;
    }
    if(pSrcImg->stVFrame.u32Width != dstW || pSrcImg->stVFrame.u32Height != dstH){
        ret = imresize(src, dst);
        // printf("%d, imresize! %s \n", __LINE__, imStrError((IM_STATUS)ret));
    }

    ret = imcvtcolor(src, dst, src.format, dst.format);

    cv::Mat mat(dstH, dstW, CV_8UC3, (void*)pDstData);
    image = mat.clone();

    free(pDstData);

    return 0;
}

int AlgoChannel::drawInfo(void* pImage, std::vector<DRESULT> results)
{
    int count = 0;
    VGS_HANDLE handle = 0;
    VGS_TASK_ATTR_S stTask = {0};
    
    if(!pImage) return -1;

    stTask.stImgOut = stTask.stImgIn = *(VIDEO_FRAME_INFO_S*)pImage;
    int imageWidth = stTask.stImgOut.stVFrame.u32Width;
    int imageHeight = stTask.stImgIn.stVFrame.u32Height;

    for(auto& result : results){
        if(result.fwidth <= 0 || result.fheight <= 0){
            continue;
        }
        lines[count].stStartPoint.s32X = (int)(result.fx * imageWidth) / 2 * 2;
        lines[count].stStartPoint.s32Y = (int)(result.fy * imageHeight) / 2 * 2;
        lines[count].stEndPoint.s32X = (int)((result.fx  + result.fwidth) * imageWidth) / 2 * 2;
        lines[count].stEndPoint.s32Y = (int)(result.fy * imageHeight) / 2 * 2;
        lines[count].u32Color = 0xFF0000;
        lines[count].u32Thick = 2;
        count ++;

        ///绘制框的左竖线
        lines[count].stStartPoint.s32X = (int)(result.fx * imageWidth) / 2 * 2;
        lines[count].stStartPoint.s32Y = (int)(result.fy * imageHeight) / 2 * 2;
        lines[count].stEndPoint.s32X = (int)(result.fx * imageWidth) / 2 * 2;
        lines[count].stEndPoint.s32Y = (int)((result.fy + result.fheight) * imageHeight)  / 2 * 2;
        lines[count].u32Color = 0xFF0000;
        lines[count].u32Thick = 2;
        count ++;

        ///绘制框的下横线
        lines[count].stStartPoint.s32X = (int)(result.fx * imageWidth) / 2 * 2;
        lines[count].stStartPoint.s32Y = (int)((result.fy + result.fheight) * imageHeight) / 2 * 2;
        lines[count].stEndPoint.s32X = (int)((result.fx + result.fwidth) * imageWidth) / 2 * 2;
        lines[count].stEndPoint.s32Y = (int)((result.fy + result.fheight) * imageHeight) / 2 * 2;
        lines[count].u32Color = 0xFF0000;
        lines[count].u32Thick = 2;
        count ++;
        
        ///绘制框的右竖线
        lines[count].stStartPoint.s32X = (int)((result.fx + result.fwidth) * imageWidth) / 2 * 2;
        lines[count].stStartPoint.s32Y = (int)(result.fy * imageHeight) / 2 * 2;
        lines[count].stEndPoint.s32X = (int)((result.fx + result.fwidth) * imageWidth) / 2 * 2;
        lines[count].stEndPoint.s32Y = (int)((result.fy + result.fheight) * imageHeight) / 2 * 2;
        lines[count].u32Color = 0xFF0000;
        lines[count].u32Thick = 2;
        count ++;
    }

    RK_S32 ret = RK_MPI_VGS_BeginJob(&handle);
    if(ret != 0){
        printf("RK_MPI_VGS_BeginJob failed! \n");
        return -1;
    }

    if(count > 0){
        ret = RK_MPI_VGS_AddDrawLineTaskArray(handle, &stTask, lines, count);
        if(ret != 0){
            printf("ss_mpi_vgs_add_draw_line_task faled! 0x%x.\n",ret);
            RK_MPI_VGS_CancelJob(handle);
            return -1;
        }
    }
    
    if(handle != -1){
        ret = RK_MPI_VGS_EndJob(handle);
        if(ret != 0){
            printf("RK_MPI_VGS_EndJob faled! \n");
            return -1;
        }
    }
}

void AlgoChannel::onProcDataCallBack() {
    // 线程的代码
    prctl(PR_SET_NAME,"AlgoChannel_ProcData");
    printf("Thread running\n");
    VIDEO_FRAME_INFO_S stFrame;
    while(!m_exit)
    {
        void* pdstMBlk = NULL;
        int iScaleWidth = 0;
        int iScaleHeight = 0;

        memset(&stFrame, 0, sizeof(VIDEO_FRAME_INFO_S));
        int iRet = RK_MPI_VPSS_GetChnFrame(0, 0, &stFrame, -1);

	if(iRet != 0)
        {
            usleep(10*1000);
            continue;
        }

        CropYuvData(stFrame.stVFrame.pMbBlk,&pdstMBlk,2960,1080);//按照2960*1080分割并重新拼图
//		printf("---------- iRet = %d\n", iRet);

        RK_MPI_VPSS_ReleaseChnFrame(0, 0, &stFrame);

//		continue;
		
        int iPicLen = RK_MPI_MB_GetLength(pdstMBlk);
        
        VIDEO_FRAME_INFO_S yuvFrameInfo = {0};//拼图后buffer

        yuvFrameInfo.stVFrame.pMbBlk = pdstMBlk;
        yuvFrameInfo.stVFrame.u32Width = 2960;
        yuvFrameInfo.stVFrame.u32Height = 3240;
        yuvFrameInfo.stVFrame.u32VirWidth = 2960;
        yuvFrameInfo.stVFrame.u32VirHeight = 3240;

        //按照模型输入大小进行等比缩放
        if(yuvFrameInfo.stVFrame.u32Width > yuvFrameInfo.stVFrame.u32Height)
        {
            iScaleWidth = m_modelWidth;
            iScaleHeight = (m_modelWidth * yuvFrameInfo.stVFrame.u32Height) / yuvFrameInfo.stVFrame.u32Width;
        }
        else
        {
            iScaleWidth = (m_modelHeight * yuvFrameInfo.stVFrame.u32Width) / yuvFrameInfo.stVFrame.u32Height;
            iScaleHeight = m_modelHeight;
            iScaleWidth = (iScaleWidth + 16) / 16 * 16;
        }

        cv::Mat iveOutFrame, dstFrameMat;

        //yuv转jpg
        yuv420ToCvMat(&yuvFrameInfo, iScaleWidth, iScaleHeight, iveOutFrame);
        
        //根据模型大小填充数据
        if(m_modelHeight > iScaleHeight)
            cv::copyMakeBorder(iveOutFrame, dstFrameMat, 0, m_modelHeight - iScaleHeight, 0, 0, cv::BORDER_CONSTANT, cv::Scalar(114, 114, 114));
        else
            cv::copyMakeBorder(iveOutFrame, dstFrameMat, 0, 0, (m_modelWidth - iScaleWidth) / 2, (m_modelWidth - iScaleWidth) / 2, cv::BORDER_CONSTANT, cv::Scalar(114, 114, 114));

        std::vector<BBOX> detectResults;
        std::vector<BBOX> calResults;

        iRet = m_yolov.detect(dstFrameMat.data, detectResults);
        if (iRet != 0) {
            printf("Detect data failed \n");
        }

        {
            std::lock_guard< std::mutex> lock(m_mutex);
            m_sendResults.clear();

            calResults = CalResult(detectResults, iScaleWidth, iScaleHeight);
            
            if(!calResults.empty()){
                for(auto& box : calResults){
                    DRESULT sendResult;
                    sendResult.fx = (float)box.rect.ltX / (float)7680;
                    sendResult.fy = box.rect.ltY / (float)1080;
                    sendResult.fwidth = (box.rect.rbX -box.rect.ltX)/ (float)7680;
                    sendResult.fheight = (box.rect.rbY -box.rect.ltY)/ (float)1080;
                    sendResult.id = box.id;
            //float fScore = box.score / 100.0f;       //结果打分
					if (box.score < 60)
						continue;
                    printf("id %d [%f, %f] width %f height %f score %f\n", sendResult.id, sendResult.fx,sendResult.fy,sendResult.fwidth,sendResult.fheight, box.score / 100.0f);
                    m_sendResults.push_back(sendResult);
                }
            }
        }
        
        RK_MPI_SYS_MmzFree(pdstMBlk);
        pdstMBlk = NULL;
	usleep(10 * 1000);
    }



    return;
}

void *AlgoChannel::setProcDataCallBack(void *user)
{
    AlgoChannel *ptr = (AlgoChannel *)user;

    if(ptr)
        ptr->onProcDataCallBack();

    return NULL;
}

void AlgoChannel::onSendDataCallBack() {
    // 线程的代码
    prctl(PR_SET_NAME,"AlgoChannel_SendData");
    printf("Thread running\n");
    VIDEO_FRAME_INFO_S stFrame;
    while(!m_exit)
    {
        memset(&stFrame, 0, sizeof(VIDEO_FRAME_INFO_S));
        int iRet = RK_MPI_VPSS_GetChnFrame(0, 0, &stFrame, -1);
		if(iRet != 0)
        {
            usleep(50 * 1000);
            continue;
        }
        {
            std::lock_guard< std::mutex> lock(m_mutex);
            drawInfo(&stFrame,m_sendResults);
        }
            

        RK_MPI_VENC_SendFrame(0,&stFrame,10);
        RK_MPI_VENC_SendFrame(1,&stFrame,10);

        if(m_cb)
        {
            std::lock_guard< std::mutex> lock(m_mutex);
            m_cb(m_sendResults);
        }

        RK_MPI_VPSS_ReleaseChnFrame(0, 0, &stFrame);
        
        usleep(10*1000);
    }

    return;
}

void *AlgoChannel::setSendDataCallBack(void *user)
{
    AlgoChannel *ptr = (AlgoChannel *)user;

    if(ptr)
        ptr->onSendDataCallBack();

    return NULL;
}

int AlgoChannel::Init(char *pucModelPath,int iModelWidth,int iModelHeight)
{
    if(pucModelPath == NULL || strlen(pucModelPath) == 0)
    {
        printf("model path null\n");
        return -1;
    }

    unsigned char* pucModelData = nullptr;
    int iModelSize = 0;
    pucModelData = LoadModel(pucModelPath, &iModelSize);
    if(pucModelData == NULL){
        printf("loadModelData failed! \n");
        return -1;
    }

    m_yolov.open(iModelWidth, iModelHeight, 8);
    if(m_yolov.init((const unsigned char *)pucModelData, iModelSize) == 0){
        m_yolov.getModelRequstSize(m_modelWidth, m_modelHeight);
    }

    free(pucModelData);

    int iRet = pthread_create(&m_phOutOSThread, NULL, AlgoChannel::setProcDataCallBack, this);
    if (iRet != 0){
        printf("create thread err\n");
        return -1;
    }
    iRet = pthread_create(&m_phSendThread, NULL, AlgoChannel::setSendDataCallBack, this);
    if (iRet != 0){
        printf("create thread err\n");
        return -1;
    }
    return 0;
}

int AlgoChannel::Destory()
{
    m_exit = true;

    pthread_detach(m_phOutOSThread);
    pthread_detach(m_phSendThread);
    return 0;
}

int AlgoChannel::setDetectDataCallback(ALGOCHANNEL_DETECTPROC cb)
{
    m_cb = cb;
    return 0;
}

AlgoChannel::AlgoChannel(/* args */)
{
}

AlgoChannel::~AlgoChannel()
{
}
